﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Systems;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.Foundation;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

using System.Threading;
using Drifting.MotionPlanning.Primitives;
using Drifting.Graphics.Visualizers;
using Drifting.MotionPlanning.SingleThreading.Planners;
using Drifting.MotionPlanning.SingleThreading.Algorithms;
using System.Diagnostics;



namespace Drifting.Scenes
{
    public class DireWolf_prototype : Basic
    {
        RenderTarget2D target_perspective;
        Rectangle rec_perspective;

        // forward propagation
        RenderTarget2D target_phaseA;
        Rectangle rec_phaseA;

        // backward propagation
        RenderTarget2D target_phaseB;
        Rectangle rec_phaseB;

        Matrix phaseA_view;
        Matrix phaseA_project;

        SpriteBatch spriteBatch;

        Vector2 canvas_horizontal;
        Vector2 canvas_vertical;
        Vector2 viewport_horizontal;
        Vector2 viewport_vertical;

        Line2D l_axis;
        Line2D l_red;
        Line2D l_blue;
        Line2D l_green;
        Tetrahedron tetra;

        Line l_A;

        OneLink_wolf onelink;
        Random rand;

        Thread action_thread;

        double support_x = 0;
        double support_y = 0;
        List<double[]> point_list_1;
        List<double[]> point_list_2;
        double[] q_wolf;
        double[] q_plus = null;
        bool A_support;
        Box boxTop;
        Box boxA;
        Box boxB;
        Box boxPointer;
        Box boxPointer2;
        double period;
        double period_left;
        double L_thigh = 0.6;
        double L_shank = 0.6;


        public DireWolf_prototype(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game, SpriteFont spriteFont)
            : base(ref view, ref projection, device, camera, game)
        {
            target_phaseA = new RenderTarget2D(
                device,
                400,
                400,
                false,
                device.PresentationParameters.BackBufferFormat,
                DepthFormat.Depth24,
                4,
                RenderTargetUsage.PreserveContents);

            rec_phaseA = new Rectangle(801, 0, 400, 400);

            phaseA_project = Matrix.CreateOrthographic(20f, 20, -1, 5);    //1.8f, 10, -1, 5
            phaseA_view = Matrix.CreateLookAt(
                new Vector3(0f, 0, 1),
                new Vector3(0f, 0, 0),
                new Vector3(0, 1, 0));


            target_phaseB = new RenderTarget2D(
                device,
                400,
                400,
                false,
                device.PresentationParameters.BackBufferFormat,
                DepthFormat.Depth24);
            rec_phaseB = new Rectangle(801, 401, 400, 400);

            target_perspective = new RenderTarget2D(
                device,
                800,
                533,
                false,
                device.PresentationParameters.BackBufferFormat,
                DepthFormat.Depth24,
                8,
                RenderTargetUsage.DiscardContents);
            rec_perspective = new Rectangle(0, 0, 800, 533);

            canvas_horizontal = new Vector2(-10, 10);
            canvas_vertical = new Vector2(-10, 10);
            viewport_horizontal = new Vector2(0f, (float)Math.PI);
            viewport_vertical = new Vector2(-10, 10);

            spriteBatch = new SpriteBatch(device);

            camera.Camera_InitView(2f, 1.2f, 5f, 2, 0, 0, (float)(Math.PI / 3.2f), ref view, ref projection);
            //grid.XYZ = false;
            grid.setGrid(20, 0.2f, 8, 0.03);

            l_axis = new Line2D(0.02f, Color.Black, device);
            l_red = new Line2D(0.02f, Color.Red, device);
            l_blue = new Line2D(0.02f, Color.Blue, device);
            l_green = new Line2D(0.2f, Color.Green, device);
            boxA = new Box(1, 0.075, 0.075, Color.LightBlue, device);
            boxB = new Box(1, 0.075, 0.075, Color.LightSalmon, device);
            boxTop = new Box(0.4, 0.6, 0.25, Color.DarkGray, device);
            l_A = new Line(0.05f, Color.Red, device);
            tetra = new Tetrahedron(1, Color.Blue, device);
            boxPointer = new Box(0.4f, 0.12f, 0.12f, Color.Blue, device);
            boxPointer2 = new Box(0.4f, 0.12f, 0.12f, Color.Red, device);

            onelink = new OneLink_wolf();
            q_wolf = new double[2];

            rand = new Random((int)DateTime.Now.Ticks);

            point_list_1 = new List<double[]>(1024);
            point_list_2 = new List<double[]>(1024);

            ThreadStart threadStart_Action = new ThreadStart(Action);
            action_thread = new Thread(threadStart_Action);
            action_thread.IsBackground = true;
            action_thread.Start();
        }


        private Matrix getConfiguration(float[] f33)
        {
            float d1 = f33[3] - f33[0];
            float d2 = f33[4] - f33[1];
            float d3 = f33[5] - f33[2];
            Vector3 v_delta = new Vector3(d1, d2, d3);
            Vector3 v_position = new Vector3(f33[0], f33[1], f33[2]);

            Vector3 v_original = new Vector3(1, 0, 0);
            float v_delta_length = v_delta.Length();

            Vector3 axis = MathAssistant.vector_cross3(v_original, v_delta);
            float sinT = axis.Length() / v_delta_length;
            axis.Normalize();
            float cosT = (float)MathAssistant.vector_dotProduct(v_original, v_delta) / v_delta_length;
            float T = MathAssistant.AngleFromSinCos(sinT, cosT);

            Matrix config =
                Matrix.CreateScale(v_delta_length, 1, 1) *
                Matrix.CreateTranslation(v_delta_length / 2, 0, 0) *
                Matrix.CreateFromAxisAngle(axis, T) *
                Matrix.CreateTranslation(v_position);

            return config;
        }

        
        private void Draw_perspective(Matrix camera, Matrix projection)
        {
            device.SetRenderTarget(target_perspective);
            device.DepthStencilState = new DepthStencilState() { DepthBufferEnable = true };
            device.Clear(Color.White);

            grid.Draw(camera, projection);

            double L_A = 1;
            double L_B = 1;
            double W_Hip = 0.4;

            double Leg_high = 0.5;
 


            double hip_x = support_x + Math.Cos(q_wolf[0]) * L_A;
            double hip_y = support_y + Math.Sin(q_wolf[0]) * L_A;

            boxTop.SetConfig(Matrix.CreateTranslation((float)hip_x, (float)hip_y, (float)0));
            boxTop.Draw(camera, projection);


            double hinge_x = 0;
            double hinge_y = 0;
            double swing_progress = 1 - period_left / period;

            if (A_support)
            {

                L_A = 1;
                double T_A = Math.Asin(W_Hip / 2 / L_A);
                DrawPendulum(boxA, 0, camera, projection, L_A, support_x, support_y, q_wolf[0] + T_A);

                hinge_x = support_x + Math.Cos(q_wolf[0]) * L_A;
                hinge_y = support_y + Math.Sin(q_wolf[0]) * L_A;

                double p = MathHelper.ToDegrees((float) Math.Acos((L_shank * L_shank - L_A * L_A - L_thigh * L_thigh) / (-2 * L_thigh * L_A)));


                if (q_plus != null)
                {
                    if (swing_progress <= 0.5)
                    {
                        L_B = (0.5 - swing_progress) / 0.5 * (1 - Leg_high) + Leg_high;
                    }
                    else
                    {
                        L_B = (swing_progress - 0.5) / 0.5 * (q_plus[2] - Leg_high) + Leg_high;
                    }
                    double swing_x = hip_x - L_B * Math.Cos(q_plus[0]);
                    double swing_y = hip_y - L_B * Math.Sin(q_plus[0]);
                    double T_B = Math.Asin(W_Hip / 2 / L_B);
                    DrawPendulum(boxB, 0, camera, projection, L_B, swing_x, swing_y, q_plus[0] - T_B);
                }

            }
            else
            {
                L_B = 1;
                double T_B = Math.Asin(W_Hip / 2 / L_B);
                DrawPendulum(boxB, 0, camera, projection, L_B, support_x, support_y, q_wolf[0] - T_B);

                hinge_x = support_x + Math.Cos(q_wolf[0]) * L_B;
                hinge_y = support_y + Math.Sin(q_wolf[0]) * L_B;

                if (q_plus != null)
                {
                    if (swing_progress <= 0.5)
                    {
                        L_A = (0.5 - swing_progress) / 0.5 * (1 - Leg_high) + Leg_high;
                    }
                    else
                    {
                        L_A = (swing_progress - 0.5) / 0.5 * (q_plus[2] - Leg_high) + Leg_high;
                    }
                    double swing_x = hip_x - L_A * Math.Cos(q_plus[0]);
                    double swing_y = hip_y - L_A * Math.Sin(q_plus[0]);
                    double T_A = Math.Asin(W_Hip / 2 / L_A);
                    DrawPendulum(boxA, 0, camera, projection, L_A, swing_x, swing_y, q_plus[0] + T_A);
                }
            }
            if (force_left)
            {
                Draw_pointer_left(camera, projection, (float)hinge_x - 0.8f, (float)hinge_y);
                if ((DateTime.Now - tm_pressed).TotalSeconds > 0.6)
                {
                    force_left = false;
                }
            }
            if (force_right)
            {
                Draw_pointer_right(camera, projection, (float)hinge_x + 0.8f, (float)hinge_y);
                if ((DateTime.Now - tm_pressed).TotalSeconds > 0.6)
                {
                    force_right = false;
                }
            }



            device.SetRenderTarget(null);
        }

        private void Draw_pointer_left(Matrix camera, Matrix projection, float pointer_x, float pointer_y)
        {
            Matrix m_pointer =
                Matrix.CreateScale(0.25f, 0.25f, 0.5f) *
                Matrix.CreateRotationZ(0) *
                Matrix.CreateRotationY((float)(Math.PI / 2)) *
                Matrix.CreateTranslation(pointer_x, pointer_y, 0);
            tetra.Draw(camera, projection, m_pointer);
            m_pointer =
                Matrix.CreateScale(0.25f, 0.25f, 0.5f) *
                Matrix.CreateRotationZ((float)Math.PI) *
                Matrix.CreateRotationY((float)(Math.PI / 2)) *
                Matrix.CreateTranslation(pointer_x, pointer_y, 0);
            tetra.Draw(camera, projection, m_pointer);
            m_pointer =
                Matrix.CreateTranslation(pointer_x - 0.3f, pointer_y, 0);
            boxPointer.Draw(camera, projection, m_pointer);
        }

        private void Draw_pointer_right(Matrix camera, Matrix projection, float pointer_x, float pointer_y)
        {
            Matrix m_pointer =
                Matrix.CreateScale(0.25f, 0.25f, 0.5f) *
                Matrix.CreateRotationZ(0) *
                Matrix.CreateRotationY((float)(Math.PI / 2 * 3)) *
                Matrix.CreateTranslation(pointer_x, pointer_y, 0);
            tetra.Draw(camera, projection, m_pointer);
            m_pointer =
                Matrix.CreateScale(0.25f, 0.25f, 0.5f) *
                Matrix.CreateRotationZ((float)Math.PI) *
                Matrix.CreateRotationY((float)(Math.PI / 2 * 3)) *
                Matrix.CreateTranslation(pointer_x, pointer_y, 0);
            tetra.Draw(camera, projection, m_pointer);
            m_pointer = Matrix.CreateTranslation(pointer_x + 0.3f, pointer_y, 0);
            boxPointer.Draw(camera, projection, m_pointer);
        }



        private void Draw_phaseB(Matrix camera, Matrix projection)
        {
            device.SetRenderTarget(target_phaseB);
            device.DepthStencilState = new DepthStencilState() { DepthBufferEnable = true };
            device.Clear(Color.GhostWhite);

            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(-10, 0, 0), new Vector3(10f, 0, 0));
            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(0, 10, 0), new Vector3(0f, -10, 0));

            device.SetRenderTarget(null);
        }
        private void Draw_phaseA(Matrix camera, Matrix projection)
        {
            device.SetRenderTarget(target_phaseA);
            device.DepthStencilState = new DepthStencilState() { DepthBufferEnable = true };
            device.Clear(Color.GhostWhite);
            
            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(-10, 0, 0), new Vector3(10f, 0, 0));
            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(0, 10, 0), new Vector3(0f, -10, 0));

            
            for (int i = 0; i < point_list_1.Count; i+=20)
            {
                draw_point_forward(point_list_1[i]);
            }

            for (int i = 0; i < point_list_2.Count; i+=20)
            {
                draw_point_backward(point_list_2[i]);
            }

            draw_point_wolf(q_wolf);

            device.SetRenderTarget(null);
        }

        public void draw_point_forward(double[] p)
        {
            l_red.Draw(phaseA_view, phaseA_project,
                            new Vector3(X_From_Q_to_phaseA(p[0] - 0.02), Y_From_Q_to_phaseA(p[1]), 0),
                            new Vector3(X_From_Q_to_phaseA(p[0] + 0.02), Y_From_Q_to_phaseA(p[1]), 0));
        }

        public void draw_point_backward(double[] p)
        {
            l_blue.Draw(phaseA_view, phaseA_project,
                            new Vector3(X_From_Q_to_phaseA(p[0] - 0.02), Y_From_Q_to_phaseA(p[1]), 0),
                            new Vector3(X_From_Q_to_phaseA(p[0] + 0.02), Y_From_Q_to_phaseA(p[1]), 0));
        }

        public void draw_point_wolf(double[] p)
        {
            l_green.Draw(phaseA_view, phaseA_project,
                            new Vector3(X_From_Q_to_phaseA(p[0] - 0.04), Y_From_Q_to_phaseA(p[1]), 0),
                            new Vector3(X_From_Q_to_phaseA(p[0] + 0.04), Y_From_Q_to_phaseA(p[1]), 0));
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            Draw_phaseA(camera, projection);
            //Draw_phaseB(camera, projection);
            Draw_perspective(camera, projection);

            spriteBatch.Begin(SpriteSortMode.Immediate, BlendState.AlphaBlend,
                SamplerState.LinearClamp, DepthStencilState.Default,
                RasterizerState.CullNone);

            spriteBatch.Draw(target_phaseA, rec_phaseA, Color.White);
            //spriteBatch.Draw(target_phaseB, rec_phaseB, Color.White);
            spriteBatch.Draw(target_perspective, rec_perspective, Color.White);


            spriteBatch.End();
        }

        private void DrawPendulum(Box box, double depth, Matrix camera, Matrix projection, double L, double x, double y, double theta)
        {
            double angle = theta;
            box.SetConfig(
                Matrix.CreateScale((float)L,1,1) * 
                Matrix.CreateTranslation((float)(L / 2), 0, 0) * 
                Matrix.CreateRotationZ((float)angle) * 
                Matrix.CreateTranslation((float)x, (float)y, (float)depth));
            box.Draw(camera, projection);
        }

        private float X_From_Q_to_phaseA(double qx)
        {
            double viewport_horizontal_range = viewport_horizontal.Y - viewport_horizontal.X;
            double canvas_horizontal_range = canvas_horizontal.Y - canvas_horizontal.X;

            return (float)((qx - viewport_horizontal.X) / viewport_horizontal_range * canvas_horizontal_range + canvas_horizontal.X);
        }
        private float Y_From_Q_to_phaseA(double qy)
        {
            double viewport_vertical_range = viewport_vertical.Y - viewport_vertical.X;
            double canvas_vertical_range = canvas_vertical.Y - canvas_vertical.X;

            return (float)((qy - viewport_vertical.X) / viewport_vertical_range * canvas_vertical_range + canvas_vertical.X);
        }

        public void Action()
        {
            double[] root = new double[] { Math.PI / 2, 0 };
            double[] u = new double[1];
            BaseSystem.MemoryPad pad = new BaseSystem.MemoryPad();
            pad.Prepare(2);

            double initial_error_dist = 0.01;
            int n_tries = 50;
            double duration = 3;
            double duration_inc = 0.005;
            // forward propagation
            for (int i = 0; i<n_tries;++i )
            {
                double[] q_start = new double[2];
                q_start[0] = rand.NextDouble() - 0.5;
                q_start[1] = rand.NextDouble() - 0.5;
                double e_l = MathAssistant.Norm(q_start);
                q_start[0] *= initial_error_dist / e_l;
                q_start[1] *= initial_error_dist / e_l;
                q_start[0] += root[0];
                q_start[1] += root[1];
                double[] q_next = new double[2];

                point_list_1.Add(q_start);

                double[] q_tmp = new double[2];
                MathAssistant.vector_set(q_start, ref q_tmp);
                for (double t = 0; t < duration; t += duration_inc)
                {
                    double step_cost = 0;
                    bool inState = onelink.Propagate(pad, q_tmp, u, duration_inc, ref q_next, ref step_cost);
                    if (inState == false)
                        break;

                    MathAssistant.vector_set(q_next, ref q_tmp);

                    double[] q_new = new double[2];
                    MathAssistant.vector_set(q_next, ref q_new);
                    point_list_1.Add(q_new);
                }

            }
            //backward propagation
            for (int i = 0; i < n_tries; ++i)
            {
                double[] q_start = new double[2];
                q_start[0] = rand.NextDouble() - 0.5;
                q_start[1] = rand.NextDouble() - 0.5;
                double e_l = MathAssistant.Norm(q_start);
                q_start[0] *= initial_error_dist / e_l;
                q_start[1] *= initial_error_dist / e_l;
                q_start[0] += root[0];
                q_start[1] += root[1];
                double[] q_next = new double[2];

                point_list_1.Add(q_start);

                onelink.Leg = 1;

                double[] q_tmp = new double[2];
                MathAssistant.vector_set(q_start, ref q_tmp);
                for (double t = 0; t < duration; t += duration_inc)
                {
                    double step_cost = 0;
                    bool inState = onelink.Propagate(pad, q_tmp, u, -duration_inc, ref q_next, ref step_cost);
                    if (inState == false)
                        break;
                    MathAssistant.vector_set(q_next, ref q_tmp);
                    double L = onelink.Adjust_Leg_To_Height(1, q_next[0]);

                    double[] q_new = new double[4];
                    MathAssistant.vector_set(q_next, ref q_new);
                    q_new[2] = L;
                    q_new[3] = q_new[1] * L * L;
                    point_list_2.Add(q_new);
                }

            }


            period = 0.6;
            period_left = period;

            
            q_wolf[0] = Math.PI / 2;
            q_wolf[1] = 0;

            q_wolf[1] -= 0.3;
            A_support = true;

            
            long tick = 0;
            DateTime last_tm = DateTime.FromBinary(tick); //DateTime.Now;
            
            while (true)
            {
                tick += 100000;
                DateTime now = DateTime.FromBinary(tick);
                double duration_i = (now - last_tm).TotalSeconds;
                period_left -= duration_i;
                
                if (period_left < 0)
                {
                    period_left = period;

                    double L = 1;

                    double dx = L * Math.Cos(q_wolf[0]);
                    double dy = 0;

                    q_wolf[0] = q_plus[0];
                    q_wolf[1] = q_plus[1];
                    A_support = !A_support;

                    dx += -q_plus[2] * Math.Cos(q_wolf[0]);
                    dy += 0;

                    support_x += dx;
                    support_y += dy;
                }
                else
                {
                    double[] q_predict = new double[2];
                    double[] q_predict_next = new double[2];
                    MathAssistant.vector_set(q_wolf, ref q_predict);

                    for (double t = 0; t < period_left; t += duration_inc)
                    {
                        double step_cost = 0;
                        bool inState = onelink.Propagate(pad, q_predict, u, duration_inc, ref q_predict_next, ref step_cost);
                        MathAssistant.vector_set(q_predict_next, ref q_predict);
                    }

                    double l_predict = 1;
                    double angular_momentum = q_predict[1] * l_predict * l_predict;
                    // search for the closest angular momenum in point_list_2

                    double e_min = double.MaxValue;
                    
                    for (int i = 0; i < point_list_2.Count; ++i)
                    {
                        double e_i = Math.Abs(angular_momentum - point_list_2[i][3]);
                        if (e_min > e_i)
                        {
                            e_min = e_i;
                            q_plus = point_list_2[i];
                        }
                    }


                
                }

                // physics simulation
                double[] q_tmp = new double[2];
                double[] q_next = new double[2];
                MathAssistant.vector_set(q_wolf, ref q_tmp);
                onelink.Leg = q_plus[2];
                for (double t = 0; t < duration_i; t += duration_inc)
                {
                    // we must add leg length here
                    onelink.Adjust_Leg_To_Height(1, q_tmp[0]);

                    double step_cost = 0;
                    bool inState = onelink.Propagate(pad, q_tmp, u, duration_inc, ref q_next, ref step_cost);
                    MathAssistant.vector_set(q_next, ref q_tmp);
                }
                MathAssistant.vector_set(q_tmp, ref q_wolf);


                last_tm = now;
                Thread.Sleep(10);
            }





        }

        bool force_right = false;
        bool force_left = false;
        DateTime tm_pressed = DateTime.Now;

        DateTime lastKeyPressed = DateTime.Now;
        override public void Update()
        {
            DateTime now = DateTime.Now;
            TimeSpan ts = (now - lastKeyPressed);

            if (ts.TotalSeconds > 0.1)
            {
                if (Keyboard.GetState(PlayerIndex.One).IsKeyDown(Keys.LeftControl))
                {
                    q_wolf[1] -= 0.2;
                    force_left = true;
                    force_right = false;
                    tm_pressed = DateTime.Now;
                }
                if (Keyboard.GetState(PlayerIndex.One).IsKeyDown(Keys.RightControl))
                {
                    q_wolf[1] += 0.2;
                    force_right = true;
                    force_left = false;
                    tm_pressed = DateTime.Now;
                }
                lastKeyPressed = now;
            }
        }
    }
}
